/*	Util.c
*
*	Useful utilities for FlutterBot.
*
*  TODO - i2c, usart, compass and the like should be broken out into thier own files.
*
*
*/

#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>
#include "util.h"
#include "button.h"
#include "LCD_driver.h"

// UART global variables
// flag variables
volatile unsigned char uartReadyTx;			///< uartReadyTx flag
volatile unsigned char uartBufferedTx;		///< uartBufferedTx flag

/*********************************************************************
*	 ADC Tests
**********************************************************************/
void showADCLeftRight(void)  
{
    unsigned char adcL, adcR;
   
    SetupLineSensors();
    InitADC();
   
    while (!(getkey() == 1))       //do the following block until enter is pressed
    {
        // get line sensorvalues
        adcL = GetADC(5);
        adcR = GetADC(4);
		show2hex(adcL, adcR);
		sendNumber(adcL);
		sendChar(',');
		sendNumber(adcR);
		sendString("\r\n");
		TimerWait(100);
		
    }
}
void showADCLineCenter(void)  
{
    unsigned char adcLine, adcM;
   
    SetupLineSensors();
    InitADC();
   
    while (!(getkey() == 1))       //do the following block until enter is pressed
    {
        // get line sensor values
		adcM = GetADC(6);
		adcLine = GetADC(7);
		show2hex(adcLine, adcM);
		sendNumber(adcM);
		sendString("\r\n");
		TimerWait(100);
		
    }
}


/*********************************************************************
*	Takes a reading of the compass and subtracts it from current heading
**********************************************************************/
void CompassDiff(void)
{
	unsigned char	BattleShip,
					InYour;
	ReadCompass();
	BattleShip = Heading;
	while (!(getkey() == 1)){
		ReadCompass();
		InYour = Heading;
		show2hex(InYour, BattleShip);
	}
}


/*********************************************************************
*	 Sees if there is noise on the system
**********************************************************************/
void ThermNoise(void)
{
	unsigned char	HotPixel,
					Temperature;
	while (!(getkey() == 1))
	{
		GetHotPixel(&HotPixel,&Temperature);
		while (Temperature<=9)
		{
			printLCD("Good");
			//TimerWait();
			//show2hex(HotPixle, Temperature);
			GetHotPixel(&HotPixel,&Temperature);			
		}
		while (1)
		{
			printLCD("Error");
			TimerWait(1000);
			show2hex(HotPixel,Temperature);
			TimerWait(1000);
		}
	}
}


/*********************************************************************
*	 CompNoise
**********************************************************************/
void CompNoise(void)
{
	unsigned char  FirstHeading;
	while (!(getkey() == 1))
	{
		ReadCompass();
		FirstHeading = Heading;
		while (1)
		{
			ReadCompass();
			if(-2 < (FirstHeading-Heading) && 2 > (FirstHeading-Heading))
			{
				printLCD("Good");
				//TimerWait();
				//show2hex(HotPixle, Temperature);	
			}
			else
			{
				while(1)
				{
					printLCD("Error");
					TimerWait(1000);
					show2hex(FirstHeading,Heading);
					TimerWait(1000);
				}
			}
		}
	}
}



/*********************************************************************
*	 Compass Test
**********************************************************************/
void CompassTest(void)
{
	while (!(getkey() == 1))
	{
		if(ReadCompass())
		{
			show1hex(Heading);
			sendNumber(Heading);
			sendString("\r\n");
		}
		else
		{
			printLCD("Error");
		}
	}
}


/*********************************************************************
*	 Flame Sensor Tests
**********************************************************************/
void FlameSensorTest(void)
{
	unsigned char	HotPixel,
					Temperature;
	
	while (!(getkey() == 1))
	{
		if(GetHotPixel(&HotPixel,&Temperature))
		{
			if(Temperature > 2)
			{
				show2hex(HotPixel,Temperature);
				sendNumber(HotPixel);
				sendChar(',');
				sendNumber(Temperature);
				sendString("\r\n");
			}
			else
			{
				printLCD("None");
			}
		}
		else
		{
			printLCD("Error");
		}
	}
}


/*********************************************************************
*	 GoToFlame
**********************************************************************/
void GoToFlame(void)						
//will go to the hot pixel
{
	unsigned char	HotPixel,
					Temperature/*,
					AvgTemp=0,
					index*/;	
	while (!(getkey() == 1))
	{
		GetHotPixel(&HotPixel,&Temperature);
	
		if(Temperature > 6)
		{
			show2hex(HotPixel,Temperature);
			sendNumber(HotPixel);
			sendChar(',');
			sendNumber(Temperature);
			sendString("\r\n");
			if (HotPixel == 1)
			{
				//printLCD("pixel 1");
				//SetRightMotorPWM(0);
				//SetLeftMotorPWM(0);
				//TimerWait(1000);
				SetRightMotorPWM(5);
				SetLeftMotorPWM(10);
				TimerWait(10);
			}
			else if (HotPixel == 2 )
			{
				//printLCD("pixel 2");
				//SetRightMotorPWM(0);
				//SetLeftMotorPWM(0);
				//TimerWait(1000);
				SetRightMotorPWM(7);
				SetLeftMotorPWM(10);
				TimerWait(10);
			}
			else if (HotPixel == 3 )
			{
			//	printLCD("pixel 3");
			//	SetRightMotorPWM(0);
			//	SetLeftMotorPWM(0);		
			//	TimerWait(1000);
				SetRightMotorPWM(8);
				SetLeftMotorPWM(10);
				TimerWait(10);
			}			
			else if (HotPixel == 4 )
			{
			//	printLCD("pixel 4");
				//SetRightMotorPWM(0);
			//	SetLeftMotorPWM(0);
			//	TimerWait(1000);
				SetRightMotorPWM(10);
				SetLeftMotorPWM(10);
				TimerWait(10);
			}						
			else if (HotPixel == 5 )
			{
			//	printLCD("pixel 5");
				//SetRightMotorPWM(0);
				//SetLeftMotorPWM(0);
				//TimerWait(1000);
				SetRightMotorPWM(10);
				SetLeftMotorPWM(10);
				TimerWait(10);
			}
			else if (HotPixel == 6)
			{
				//printLCD("pixel 6");
				//SetRightMotorPWM(0);
				//SetLeftMotorPWM(0);
				//TimerWait(1000);
				SetRightMotorPWM(10);
				SetLeftMotorPWM(9);
				TimerWait(10);
			}						
			else if (HotPixel == 7)
			{
				//printLCD("pixel 7");
				//SetRightMotorPWM(0);
				//SetLeftMotorPWM(0);
				//TimerWait(1000);
				SetRightMotorPWM(10);
				SetLeftMotorPWM(7);
				TimerWait(10);
			}		
			else if (HotPixel == 8)
			{
				//printLCD("pixel 8");
				//SetRightMotorPWM(0);
				//SetLeftMotorPWM(0);
				//TimerWait(1000);
				SetRightMotorPWM(10);
				SetLeftMotorPWM(5);
				TimerWait(10);
			}

			if(PINB & 0x20)				// check for balloon pop
			{
				SetRightMotorPWM(0);
				SetLeftMotorPWM(0);
				printLCD("Popped");
				TimerWait(1000);
				return;
			}
		}
		else
		{
			SetRightMotorPWM(0);
			SetLeftMotorPWM(0);
			printLCD("Error");
		}
	}
}


/*****************************************************************************************
*	 usart init and PC comunication functions (this is copied from Joe Pardue's Book)    *
******************************************************************************************/
char isCharAvailable()
{	
	// Does the RX0 bit of the USART Status and Control Register
	// indicate a char has been received?
	if ( (UCSRA & (0x80)) ) return 1;
	else return 0;
}


char receiveChar()
{
	// Return the char in the UDR0 register
	return UDR;
}


void sendChar(char data)
{
	// wait for the transmitter to be ready
	while(!uartReadyTx);
	// send byte
	UDR = data;
	// set ready state to FALSE
	uartReadyTx = FALSE;
}


void sendNumber(long Number)
{
	char	Result[12],
			negative	= FALSE;


	if(Number < 0)
	{
		Number		= 0 - Number;
		negative	= TRUE;
	}
	else if(!Number)
	{
		sendChar('0');
	}
	else
	{
		int		index		= 0;

		while(Number)
		{
			Result[index++]	= (Number % 10) + '0';
			Number			= Number / 10;
		}

		if(negative)
		{
			sendChar('-');
		}

		while(index)
		{
			sendChar(Result[--index]);
		}
	}
}


void sendString(char s[])
{
	int i = 0;
	
	while(i < 64) // don't get stuck if it is a bad string
	{
		if( s[i] == '\0' ) break; // quit on string terminator
		sendChar(s[i++]);
	}
}


void USART_Init(unsigned char baud)
{

//	USARTinit(103);			// Initialize USART 9.6Kb
//	USARTinit(51);			// Initialize USART 19.2Kb
//	USARTinit(25);			// Initialize USART 38.4Kb
//	USARTinit(3);			// Initialize USART 57.6Kb (2.1% error)

    // Set the USART baudrate registers for 19200 @ 8 MHz
    UBRRH = 0;			//(unsigned char)(baudrate>>8);
    UBRRL = baud;		//(unsigned char)baudrate;

    // Enable 2x speed change
    UCSRA = (1<<U2X);

    // Enable receiver and transmitter, TX interrupts
    UCSRB = (1<<RXEN)|(1<<TXEN)|(0<<RXCIE)|(1<<TXCIE)|(0<<UDRIE);

    // Set the USART to asynchronous at 8 bits no parity and 1 stop bit
    UCSRC = (0<<UMSEL)|(0<<UPM0)|(0<<USBS)|(3<<UCSZ0)|(0<<UCPOL);
	
	// initialize states
	uartReadyTx = TRUE;
	uartBufferedTx = FALSE;

	// enable interrupts
	sei();
}


// UART Transmit Complete Interrupt Handler
ISR(USART0_TX_vect)
{
	uartReadyTx = TRUE;
}



/*********************************************************************
*	GetHotPixel()
**********************************************************************/
int GetHotPixel(unsigned char *PixelNumber,unsigned char *DeltaTemp)
{
	unsigned char	highindex,hightemp,index;

	for(index=0;index<9;++index)
	{
		if(!Readthermalarray(index))
		{
			*PixelNumber	= 0;
			*DeltaTemp		= 0;
			return(FALSE);
		}
	}
			
	highindex	= 0;
	hightemp	= ambient;
	
	for(index=0;index<8;++index)
	{
		if(PixelTemperature[index] > hightemp)
		{
			highindex	= index + 1;
			hightemp	= PixelTemperature[index];
		}
	}

	*DeltaTemp		= hightemp ? hightemp - ambient : 0;	/* '?' is shortcut for if(hightemp) to assign DeltaTemp */
	*PixelNumber	= highindex;
	return(TRUE);
		
} /* GetHotPixel() */



/*********************************************************************
*	 Readthermalarray
**********************************************************************/
int Readthermalarray(unsigned char index)  
{
	cli();				// disable global interrupt 
    I2C_Start();

	if(I2C_Transmit(0xd0))
	{
		if(I2C_Transmit(index + 1))
		{
			I2C_Start();

			if(I2C_Transmit(0xd1))
			{
				if(!index)
				{
					if(I2C_Receive(&ambient, 1))
					{
						I2C_Stop();
						sei();			// enable global interrupt
						return(TRUE);
					}
				}
				else if(index <= 8)
				{
					if(I2C_Receive(&PixelTemperature[index - 1],1))
					{
						I2C_Stop();
						sei();			// enable global interrupt
						return(TRUE);
					}
				}
			}
		}
	}

	I2C_Stop();
	sei();			// enable global interrupt
	return(TRUE);
}


/*********************************************************************
*	 ReadCompass Stuff
**********************************************************************/
int ReadCompass(void)  
{
	cli();				// disable global interrupt 
    I2C_Start();

	if(I2C_Transmit(0xc0))
	{
		if(I2C_Transmit(0x01))
		{
			I2C_Start();

			if(I2C_Transmit(0xc1))
			{
				if(I2C_Receive(&Heading, 1))
				{
					I2C_Stop();
					sei();			// enable global interrupt
					return(TRUE);
				}
			}
		}
	}

	I2C_Stop();
	sei();			// enable global interrupt
	return(TRUE);
}


/*********************************************************************
*	 I2C low level functions
**********************************************************************/
void I2C_Init(void)
{
	ClearBit(DDRE, SCL);    // make it an input
	ClearBit(DDRE, SDA);    // make it an input
	ClearBit(PORTE, SCL);    // make its port value low
	ClearBit(PORTE, SDA);    // make its port value low
}

void I2C_Start(void)
{
	SCL_High();
	SDA_Low();
	SDA_Low();
	SCL_Low();
}

void I2C_Stop(void)
{
	SDA_Low();
	SDA_Low();
	SCL_High();
	SDA_High();
}

int I2C_Transmit(unsigned char TxData)
{
	unsigned char	BitCount;
	int i;

	// Send 8 bits 
	for(BitCount=0;BitCount<8;++BitCount)
	{
		if(TxData & 0x80)
		{
			SDA_High();
		}
		else
		{
			SDA_Low();
		}
		SCL_High();
		
		i	= I2C_TIMEOUT;
		
		// Wait for slow slave 
		while(!(PINE & (0x01<<SCL)))
		{
			if(!--i)
			{
				SCL_Low();
				return(FALSE);
			}
		}

		SCL_Low();
		TxData <<= 1;
	}
	
	// release bus
	SDA_High();
	SCL_High();
	
	// wait for ack from slave
	if(PINE & (0x01<<SDA))
	{
		SCL_Low();
		return(FALSE);
	}
	
	SCL_Low();
	return(TRUE);
}


int I2C_Receive(unsigned char *RxData, unsigned int last)
{
	unsigned char BitCount;
	int i;

	// recieve 8 bits
	for(BitCount=0;BitCount<8;++BitCount)
	{
		*RxData <<= 1;	// point to next bit		
		SCL_High();		// new clock 		
		
		// Wait for slow devices keeping SCL low
		i = I2C_TIMEOUT;
		while(!(PINE & (0x01<<SCL)))
		{
			if(!--i)
			{
				return(-2);
			}
		}
		
		if(PINE & (0x01<<SDA))
		{
			*RxData |= 0x01;	// set bit to 1 else leave 0
		}

		SCL_Low();	// end clock 		
	}
	_delay_ms(1);


	//If last byte set NAK else ACK
	if (last) 
	{
		SDA_High();		// Set NAK
	}
	else 
	{
		SDA_Low();		// Set ACK
	}


	// Write ACK/NAK bit
	SCL_High();
	i = I2C_TIMEOUT;
	while(!(PINE & (0x01<<SCL)))	// Wait for slow slave 
	{
		if(!--i)
		{
			SCL_Low();
			return(FALSE);
		}
	}
	SCL_Low();



/*
	SCL_Low();
	SCL_Low();
	SCL_High();
*/
	
	/* Wait for ACK Seq to end? */
/*	if(PINE & (0x01<<SDA))
	{
//		*RxData = 0x07;
		return(FALSE);
	}
*/	
	return(TRUE);

/*

	// Check if slave released SDA
	if(!(PINE & (0x01<<SDA)))
	{
		// SDA was not released by slave
		*RxData = 0x07;
		return(FALSE);
	}

	SDA_Low();	//	Take SDA		
	
	SCL_High();	// Toggle SCL	
	SCL_Low();
	
	SDA_High();	// Release SDA	


	if (last) 
	{
		SDA_High();		// Set NAK
	}
	else 
	{
		SDA_Low();		// Set ACK
	}
	SCL_Low();
		
	SDA_High();		// leave with SDA high


//	SCL_Low();
//	SCL_High();
//
#endif 
	
	return(TRUE);
*/

}



/*********************************************************************
*	 GPIO14 - todo, add error handling
**********************************************************************/
#define GPIO_ADDR	0x40

int gpio_write(char reg, char data)  
{
	cli();				// disable global interrupt 
    I2C_Start();

	if (I2C_Transmit(GPIO_ADDR))			// address is 40
	{
		if (I2C_Transmit(reg))		// set register
		{
			if (I2C_Transmit(data))	// write data 
			{
				I2C_Stop();
			}
		}
	}

	I2C_Stop();
	sei();			// enable global interrupt
	return(TRUE);
}


unsigned char gpio_read1(char reg)  
{
	unsigned char data;
	
	cli();				// disable global interrupt 
    I2C_Start();

	if(I2C_Transmit(GPIO_ADDR))
	{
		if(I2C_Transmit(reg))
		{
			I2C_Start();

			if(I2C_Transmit(GPIO_ADDR +1))
			{
				if(I2C_Receive(&data, 1))
				{
					I2C_Stop();
					sei();			// enable global interrupt
					return(data);
				}
			}
		}
	}

	I2C_Stop();
	sei();			// enable global interrupt
	return(data);
}


unsigned int gpio_read2(char reg)  
{
	unsigned char data = 0;
	unsigned int result = 0;
	
	cli();				// disable global interrupt 
    I2C_Start();

	if(I2C_Transmit(GPIO_ADDR))
	{
		if(I2C_Transmit(reg))
		{
			I2C_Start();

			if(I2C_Transmit(GPIO_ADDR +1))
			{
				I2C_Receive(&data, 0);
				result = data<<8;
					
				I2C_Receive(&data, 1);
				result += data;

				I2C_Stop();
				sei();			// enable global interrupt
				return(result);
			}
		}
	}

	I2C_Stop();
	sei();			// enable global interrupt
	return(result);
}



/*********************************************************************
*	 PCF8574 - I/O Expander (Currently just sets bits)       
**********************************************************************/
int PCF8574_write(unsigned char pins)  
{
	cli();				// disable global interrupt 
    I2C_Start();

	if(I2C_Transmit(0x40))
	{
		if(I2C_Transmit(pins))
		{
			I2C_Stop();
			sei();			// enable global interrupt
			return(TRUE);
		}
	}

	I2C_Stop();
	sei();			// enable global interrupt
	return(FALSE);
}

/*********************************************************************
*	EEPROM - Write       
**********************************************************************/
int EEPROM_write(unsigned char pins)  
{
	cli();				// disable global interrupt 
    I2C_Start();

	if(I2C_Transmit(0xA0))
	{
		if(I2C_Transmit(pins))
		{
			I2C_Stop();
			sei();			// enable global interrupt
			return(TRUE);
		}
	}

	I2C_Stop();
	sei();			// enable global interrupt
	return(FALSE);
}


/*********************************************************************
*	DS1803_write
**********************************************************************/
int DS1803_write(unsigned char pot, unsigned char value)  
{

	if(pot)
		pot=0xAA;
	else
		pot=0xA9;

	cli();				// disable global interrupt 
    I2C_Start();

	if(I2C_Transmit(0x50))
	{
		if(I2C_Transmit(pot))
		{
			if(I2C_Transmit(value))
			{
				I2C_Stop();
				sei();			// enable global interrupt
				return(TRUE);
			}
		}
	}

	I2C_Stop();
	sei();			// enable global interrupt
	return(TRUE);
}


/*********************************************************************
*	Analog-to-Digital Converter (ADC) 
**********************************************************************/
void InitADC(void) 
{
	
//    ADMUX	= (1<<ADLAR) | 0x04;					// select external AREF, left adjusted result, mux input 4
    ADMUX	= 0x04;					// select external AREF, left adjusted result, mux input 4
	ADCSRA	= (1<<ADPS2)| (1<<ADPS1) | (0<<ADPS0)	// set ADC prescaler to 64, 8MHz / 64 = 128kHz   
			| (1<<ADEN);							// Enable ADC
}


/*********************************************************************
*	Reads ADC channel (0-7) and returns a value (0-1023)
**********************************************************************/
int GetADC10(char ad_mux)
{	
	ADMUX &= ~0x1F;				// clear channel selection (low 5 bits)
	ADMUX |= ad_mux;			// select specified channel

	SetBit(ADCSRA, ADSC);		// ADC start conversion

	/* wait for conversion to complete */
	while (bit_is_clear(ADCSRA, ADIF))
		;

	return (ADC);  
}


/*********************************************************************
*	Reads ADC channel (0-7) and returns a value (0-255)
**********************************************************************/
unsigned char GetADC(char adc) 
{
	ADMUX &= ~0x1F;		 	// clear channel selection (low 5 bits)
	ADMUX |= adc;			// select specified channel

	SetBit(ADCSRA, ADSC);	// ADC start conversion

	/* wait for conversion to complete */
	while (bit_is_clear(ADCSRA, ADIF))
		;

	return (unsigned char) ADCH;
}


/*********************************************************************
*	 SetupLineSensors
**********************************************************************/
void SetupLineSensors(void)
{
   // need to disable digital inputs on portF pins we are using as adc.
   DIDR0 = (1<<ADC4D) | (1<<ADC5D);  // just using adc4 & adc5 for now
    
   // prob don't need to set adc in as input, but can't hurt.  
   // Bits 4 & 5 are the "eyes", whether we read digitaly, or via adc.
   ClearBit(DDRF, DDF4);    // bits 4 & 5 are inputs  
   ClearBit(DDRF, DDF5);   
   ClearBit(DDRF, DDF6);
   ClearBit(DDRF, DDF7);    
   SetBit(PORTF, PF4);      // pullup's on bit 4 & 5
   SetBit(PORTF, PF5);      
   SetBit(PORTF, PF6);    
   SetBit(PORTF, PF7);    

   // Turn on port F bit 3, it's used as ADC Ref voltage.  
   // (it also powers thermistor, light sensor, and volt reader, but we can't help that.)
   SetBit(PORTF, PF3);    // turn bit "on" 
   SetBit(DDRF, DDF3);    // make it an output  
}


/************************************************************************************
*   Initialize Timers for Servo
************************************************************************************/
void init_servo(void) 
{ 	
   // PWM mode 14 (fast PWM, TOP=ICR1) 
   TCCR1A |= (1<<WGM11); 
   TCCR1B |= (1<<WGM13) | (1<<WGM12); 
    
   // start timer 1 (prescale=0)    2Mhz
   TCCR1B |= (1<<CS10); 
    
   // CLEAR OC1A & OC1B pin 
   TCCR1A |= (1<<COM1A1); 
// TCCR1A |= (1<<COM1B1); 
    
   // set register values 
   ICR1=40000;     // => 50Hz (T=20ms)  2Mhz/50Hz  
   OCR1A=3000;     // => 1.5ms 
// OCR1B=3000;     // => 1.5ms 
    
   // OC1A pin = outout 
   DDRB |= (1<<DDB5);      // Also tied to the Piezo (speaker). 
// DDRB |= (1<<DDB6);       // Also tied to the Joystick Pin A (Up I think). 
} 

/************************************************************************************
*   Interrupt Software Pulse Width Modulation and Timer Stuff
************************************************************************************/
static volatile unsigned long s_msTimer = 0;

static volatile int Left_PWM_Counter = 0;
static volatile int Right_PWM_Counter = 0;
static volatile int Left_PWM_Setting = 0;
static volatile int Right_PWM_Setting = 0;
static volatile int PWM_Loop = 0;
static volatile int ISR_Cntr = 0;

// PB2 & PB3 control forward/reverse - Set Direction and make sure it's off
#define SetupLDir()	( SetBit(DDRB, DDB2), ClearBit(PORTB, PB2) )  
#define SetupRDir()	( SetBit(DDRB, DDB3), ClearBit(PORTB, PB3) )

// PB0 & PB1 for SoftWare PWM - Set Direction and make sure it's off
#define SetupLPWM()	( SetBit(DDRB, DDB0), ClearBit(PORTB, PB0) )
#define SetupRPWM()	( SetBit(DDRB, DDB1), ClearBit(PORTB, PB1) )

// define direction port & bit settings
#define LFwd()		ClearBit(PORTB, PB2)
#define LRev()		  SetBit(PORTB, PB2)
#define RFwd()		ClearBit(PORTB, PB3) 
#define RRev()		  SetBit(PORTB, PB3)

// PWM values can range from 10 (full-speed forward) to -10 (full-speed reverse) and 0 as stop. 
void SetLeftMotorPWM(int pwm)
{
	if (pwm > 10)
		pwm = 10;

	if (pwm < -10)
		pwm = -10;

	if (pwm >= 0)
	{
		LFwd();
	}
	else
	{
		LRev();
		pwm = pwm + 10;
	}
	
	Left_PWM_Setting = pwm;
}

void SetRightMotorPWM(int pwm)
{
	if (pwm > 10)
		pwm = 10;

	if (pwm < -10)
		pwm = -10;


	if (pwm >= 0)
	{
		RFwd();
	}
	else
	{
		RRev();
		pwm = pwm + 10;
	}

	Right_PWM_Setting = pwm;
}


/*****************************************************************************************
*	Timer/Counter 0 (for timing and pwm )
******************************************************************************************/

// sets up Timer/Counter 0 
void InitTimer()
{
	SetupLPWM();    // Setup pin for Output
	SetupRPWM();    // Setup pin for Output
	SetupLDir();    // Setup pin for left motor forward/Reverse
	SetupRDir();    // Setup pin for right motor forward/Reverse
	
	TCNT0 = 0;		// start counting at 0

	// clock rate = 8Mhz/64 = 128KHz  
	// CTC (Clear Timer on Compare Match) waveform generation mode
	// OCR0 (top/clear value) = 8 for match every 1ms
	TCCR0A = (1<<WGM01) | (0<<CS02) | (1<<CS01) | (1<<CS00);  //CTC, clkio/64  128KHz   
	OCR0A = 63;  // set to .5ms     

    TIMSK0 = (1<<OCIE0A);	// set Compare Match Interrupt Enable for timer/counter 0
	sei();					// set I-bit in SREG, global enable for interrupts
}

// returns number of ms since Timer started
unsigned long TimerMsCur()
{
	char sregSav;
	unsigned long ms;

	/* disable (delay) interrupts while we read
	   s_msTimer since it's 4 bytes and we don't
	   want it changing during the read! */
	sregSav = SREG;	// cache register holding Global Interrupt Flag
	cli();			// clear Global Interrupt Flag
	ms = s_msTimer;
	SREG = sregSav;	// restore register holding Global Interrupt Flag

	return ms;
}

/* returns true if the specified number of milliseconds
   has passed since the start time */
char TimerCheck(unsigned long msStart, unsigned int msWait)
{
	return TimerMsCur() - msStart > msWait;
}

/* pauses for the specified number of milliseconds */
void TimerWait(unsigned int ms)
{
	unsigned long msStart;

	msStart = TimerMsCur();
	while (!TimerCheck(msStart, ms))
		;
}

/* this interrupt happens when TCNT0 reaches OCR0 */
ISR(TIMER0_COMP_vect)
//ISR(SIG_OUTPUT_COMPARE0)
{
	if (ISR_Cntr)
	{
		--ISR_Cntr;
	}
	else
	{
		++s_msTimer;
		ISR_Cntr = 1;
	}
	
	if (PWM_Loop) 
    { 
		--PWM_Loop;
		if (Left_PWM_Counter)
		{
			--Left_PWM_Counter;
			SetBit(PORTB, PB0);
		}
		else
		{
			ClearBit(PORTB, PB0);
		}

		if (Right_PWM_Counter)
		{
			--Right_PWM_Counter;
			SetBit(PORTB, PB1);
		}
		else
		{
			ClearBit(PORTB, PB1);
		}
	}
	else
	{
		PWM_Loop = 10;
		Left_PWM_Counter = Left_PWM_Setting;
		Right_PWM_Counter = Right_PWM_Setting;
	}
}


// Disable JTAG port, making PF[4-7] available for other use.
void disable_JTAG(void)
{
    uint8_t save_status = SREG;   // save flags, in particular the global int enable flag
    cli();                        // make sure int disabled
    MCUCR |= (1<<JTD);            // set jtag disable bit in MCU control reg
    MCUCR |= (1<<JTD);            // need to set twice in row for it to take effect (hope gcc won't optimize out...)
    SREG = save_status;           // restore global int enable to whatever it was before.
}

/*****************************************************************************
*
*   Function name : OSCCAL_calibration
*
*   Returns :       None
*
*   Parameters :    None
*
*   Purpose :       Calibrate the internal OSCCAL byte, using the external 
*                   32,768 kHz crystal as reference.
*                   NOTE: The internal RC clock is automatically calibrated
*                   to be within 10% of 8MHz.  This should make it within 2%.
*                   iow, without OSCCAL it should be between 7.2  & 8.8  MHz,
*                      and after OSCCAL it should be between 7.84 & 8.16 MHz.
*
*****************************************************************************/
void OSCCAL_calibration(void)
{
    unsigned char calibrate = FALSE;
    int temp;
    unsigned char tempL;
	
    CLKPR = (1<<CLKPCE);                // set Clock Prescaler Change Enable
    CLKPR = (1<<CLKPS1) | (1<<CLKPS0); // set prescaler = 8
 
    TIMSK2 = 0;             //disable OCIE2A and TOIE2

    ASSR = (1<<AS2);        //select asynchronous operation of timer2 (32,768kHz)
    
    OCR2A = 200;            // set timer2 compare value 

    TIMSK0 = 0;             // delete any interrupt sources
        
    TCCR1B = (1<<CS10);     // start timer1 with no prescaling
    TCCR2A = (1<<CS20);     // start timer2 with no prescaling

    while((ASSR & 0x01) | (ASSR & 0x04));       //wait for TCN2UB and TCR2UB to be cleared

    Delay(1000);    // wait for external crystal to stabilise
    
    while(!calibrate)
    {
        cli(); // mt __disable_interrupt();  // disable global interrupt
        
        TIFR1 = 0xFF;   // delete TIFR1 flags
        TIFR2 = 0xFF;   // delete TIFR2 flags
        
        TCNT1H = 0;     // clear timer1 counter
        TCNT1L = 0;
        TCNT2 = 0;      // clear timer2 counter
           
        while ( !(TIFR2 & (1<<OCF2A)) );   // wait for timer2 compareflag

        TCCR1B = 0; // stop timer1

        sei(); // enable global interrupt
    
        if ( (TIFR1 & (1<<TOV1)) )
        {
            temp = 0xFFFF;      // if timer1 overflows, set the temp to 0xFFFF
        }
        else
        {   // read out the timer1 counter value
            tempL = TCNT1L;
            temp = TCNT1H;
            temp = (temp << 8);
            temp += tempL;
        }
    
        if (temp > 6250)
        {
            OSCCAL--;   // the internRC oscillator runs to fast, decrease the OSCCAL
        }
        else if (temp < 6120)
        {
            OSCCAL++;   // the internRC oscillator runs to slow, increase the OSCCAL
        }
        else
            calibrate = TRUE;   // the interRC is correct

        TCCR1B = (1<<CS10); // start timer1
    }

    CLKPR = (1<<CLKPCE);                 // set Clock Prescaler Change Enable
	CLKPR = (0<<CLKPS1) | (0<<CLKPS0);  // 8Mhz=0, 4Mhz=1, 2Mhz=2, 1Mhz=3
}


// This is used by OSCCAL
void Delay(unsigned int millisec)
{
	uint8_t i;

	while (millisec--) {
		for (i=0; i<125; i++) {
			asm volatile ("nop"::);
		}
	}
}


void Initialization(void)
{   
    OSCCAL_calibration();	// calibrate the OSCCAL byte

    InitTimer();			// Initialize 1ms timer & PWM
	
    ACSR = (1<<ACD);		// ACD = analog comparator disable bit
    DIDR0 = (7<<ADC0D);	// Disable Digital input on PF0-2 (power save)
 
    Button_Init();			// Initialize pin change interrupt on joystick
    LCD_Init();				// initialize the LCD
	disable_JTAG();			// Disable JTAG port, making pins available use. 
}
